#ifndef LIDAR_CELLS_GRID_H_
#define LIDAR_CELLS_GRID_H_
#include <math.h>
#include <iomanip>
#include <vector>
#include <chrono>
#include "common/algorithm_type/all_type.h"
#include "rclcpp/rclcpp.hpp"
namespace perception
{
    namespace algorithm
    {

        class LidarCellsGrid
        // class ConditionalFilter
        {
        public:
            // using Ptr = pcl::PointCloud<T1>::Ptr;
            // using ConstPtr = pcl::PointCloud<T1>::ConstPtr;
            LidarCellsGrid(cell_config &config)
            {
                this->cell_config_ = config;
                this->col = (int)(config.ymax - config.ymin) / config.cell_size_y;
                this->row = (int)(config.xmax - config.xmin) / config.cell_size_x;
            }
            ~LidarCellsGrid() {}

            int Grid(int x, int y, float pz, float px, float py, std::vector<std::vector<data_cell>> &data_cell_);
            int PointsCa(int i, int j, std::vector<std::vector<data_cell>> &data_cell_, std::vector<std::vector<data_cell>> &data_cell_tmp);
            int DistValid(int x, int y, std::vector<std::vector<data_cell>> &data_cell_);
            int GetConfidence(int x, int y, std::vector<std::vector<data_cell>> &data_cell_, std::vector<std::vector<data_cell>> &data_cell_tmp);
            int GetDensity(int x, int y, std::vector<std::vector<data_cell>> &data_cell_);
            int GetIntercept(int x, int y, std::vector<std::vector<data_cell>> &data_cell_);
            int GetValid(int x, int y,int search_scope, std::vector<std::vector<data_cell>> &data_cell_);// 用于判断高于地面许多的点云是否聚类为障碍物栅格
            int PushCell(std::vector<std::vector<data_cell>> &data_cell_, std::vector<std::vector<data_cell>> &data_cell_tmp, cell_config &config);
            int MergeDataCell(std::vector<std::vector<std::vector<data_cell>>> &data_cell_input, std::vector<std::vector<data_cell>> &data_cell_out);

        private:
            cell_config cell_config_;
            int col;
            int row;
        };
        // class ConditionalFilterF : public LidarPreprocessF
        // {
        // public:
        //     void LidarPreprocess *StartProcess()
        //     {
        //         return new ConditionalFilter;
        //     }
        //     ~ConditionalFilterF(){};
        // };

    }
}
#endif